/***************************************************************************
 *  Filename: dronemodeller.cpp
 *  Class: DroneModeller
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/


#include "dronemodeller.h"
#include <QTimer>
#include <QDateTime>
#include <QDebug>

DroneModeller::DroneModeller(QObject *parent) :
    QObject(parent), input(stdin), output(stdout)
{
    fileName = "steady.csv";
    testStartTime = QDateTime::currentMSecsSinceEpoch();
    //    input = QTextStream(stdin);
    //    output = QTextStream(stdout);
}

void DroneModeller::run(){
    output << "Chose your next action:" << endl;
    output << "T: Take off" << endl;
    output << "L: Land" << endl;
    output << "P: Pitch model test" << endl;
    output << "R: Roll model test" << endl;
    output << "G: Gaz (vert. speed) model test" << endl;
    output << "Y: Yaw model test" << endl;
    output << "E: Emergency" << endl;
    output << "X: Exit" << endl;
    output << "S: Stop flying" << endl;
    output << "WARNING: Exiting does not handle landing procedures. Be sure to land the drone before exiting." << endl;
    uinput = input.readLine();
    if(!uinput.compare("x", Qt::CaseInsensitive)){
        output << "Exiting..." << endl;
        emit(exit());
    }else if(!uinput.compare("t", Qt::CaseInsensitive)){
        output << "Taking off..." << endl;
        takeOff();
        output << "Wait for drone to be flying..." << endl;
        input.readLine();
    }else if(!uinput.compare("l", Qt::CaseInsensitive)){
        output << "Landing..." << endl;
        land();
        output << "Wait for drone to land..." << endl;
    }else if(!uinput.compare("p", Qt::CaseInsensitive)){
        pitchModel();
    }else if(!uinput.compare("r", Qt::CaseInsensitive)){
        rollModel();
    }else if(!uinput.compare("g", Qt::CaseInsensitive)){
        gazModel();
    }else if(!uinput.compare("y", Qt::CaseInsensitive)){
        yawModel();
    }else if(!uinput.compare("e", Qt::CaseInsensitive)){
        emit(emergency());
    }else if(!uinput.compare("s", Qt::CaseInsensitive)){
        stop();
    }else if(!uinput.compare("f", Qt::CaseInsensitive)){
        fly2Pos();
    }else if(!uinput.compare("Z", Qt::CaseInsensitive)){
        emit reset();
    }else{
        stop();
    }
    QTimer::singleShot(100, this, SLOT(run()));
}

void DroneModeller::pitchModel(){
    output << "Starting Pitch Modelling" << endl;
    output << "Give desired pitch as a percentage of maximum: (0-100 integer): " << endl;
    uinput = input.readLine();
    float desiredPitch = uinput.toDouble()/100.0;
    emit(changeFile("pitchmodel.csv"));
    output << "Press <RETURN> to start flying" << endl;
    input.readLine();
    emit fly(0, desiredPitch, 0, 0);
    output << "The drone is flying." << endl;
}

void DroneModeller::rollModel(){
    output << "Starting Roll Modelling" << endl;
    output << "Give desired roll as a percentage of maximum: (0-100 integer): " << endl;
    uinput = input.readLine();
    float  desiredRoll = uinput.toDouble()/100.0;
    emit(changeFile("rollmodel.csv"));
    qDebug() << "Starting roll model";
    output << "Press <RETURN> to start flying" << endl;
    input.readLine();
    emit fly(desiredRoll, 0, 0, 0);
    output << "Desired roll is" << desiredRoll;
    output << "The drone is flying." << endl;
}

void DroneModeller::yawModel(){
    output << "Starting Yaw Modelling" << endl;
    output << "Give desired yaw as a percentage of maximum: (0-100 integer): " << endl;
    uinput = input.readLine();
    float  desiredYaw = uinput.toDouble()/100.0;
    emit(changeFile("yawmodel.csv"));
    output << "Press <RETURN> to start flying" << endl;
    input.readLine();
    emit fly(0, 0, desiredYaw, 0);
    output << "The drone is flying." << endl;
}

void DroneModeller::gazModel(){
    output << "Starting gaz Modelling" << endl;
    output << "Give desired gaz as a percentage of maximum: (0-100 integer): " << endl;
    uinput = input.readLine();
    float  desiredGaz = uinput.toDouble()/100.0;
    emit(changeFile("gazmodel.csv"));
    testStartTime = QDateTime::currentMSecsSinceEpoch();
    output << "Press <RETURN> to start flying" << endl;
    input.readLine();
    emit fly(0, 0, 0, desiredGaz);
    output << "The drone is flying." << endl;
}

void DroneModeller::stop(){
    emit(fly(0,0,0,0));
    emit(changeFile("steady.csv"));
}

void DroneModeller::fly2Pos(){
    output << "Starting Roll Modelling" << endl;
    output << "Give desired distance X: (Integer, centimeters): " << endl;
    uinput = input.readLine();
    float desiredX = uinput.toDouble()/100.0;
    output << "Give desired distance Y: (Integer, centimeters): " << endl;
    uinput = input.readLine();
    float  desiredY = uinput.toDouble()/100.0;
    output << "Give desired distance Z: (Integer, centimeters): " << endl;
    uinput = input.readLine();
    float  desiredZ = uinput.toDouble()/100.0;
    qDebug() << "[DroneModeller] Flying to relative position";
    output << "Press <RETURN> to start flying" << endl;
    input.readLine();
    emit startController();
    emit newTarget(desiredX, desiredY, desiredZ, 0.0);
    output << "Desired position in meters is: (" << desiredX << "," << desiredY  <<  "," <<desiredZ << ")";
    output << "The drone is flying." << endl;
}
